#include "motion.h"
#include "base.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#define TAO_SHOW 0

#if TAO_SHOW//show

#include "cv.h"
#include "highgui.h"
#define max(a,b) ((a) > (b) ? (a) : (b))
#define min(a,b) ((a) < (b) ? (a) : (b))

#endif



unsigned char *motionprev = 0 ;
unsigned char *diffimage = 0 ;
unsigned char *simper = 0 ;




//int  initmotion(int width ,int height  ,MemInstance *meminstance) 
//{	
//
//	motionprev = (unsigned char *)SDRAMAlloc(width *height , meminstance); 
//	diffimage = (unsigned char *)SDRAMAlloc(width *height , meminstance); 
//	simper = (unsigned char *)SDRAMAlloc(width *height , meminstance); 
//	
//	memset(diffimage ,0 ,width *height ) ;
//
//	return 0 ;
//}


//int  initmotion(int width ,int height ) 
//{	
//	motionprev = (unsigned char *)malloc(2*width*height*sizeof(unsigned char) ); 
//	diffimage = (unsigned char *)malloc(2*width*height*sizeof(unsigned char) ); 
//	simper = (unsigned char *)malloc(2*width*height*sizeof(unsigned char) );
//
//	memset(diffimage ,0 ,2*width *height*sizeof(unsigned char) ) ;
//
//	return 0 ;
//}


int  initmotion( int width , int height ) 
{	
	motionprev = (unsigned char *)malloc(800*800*sizeof(unsigned char) ); 
	diffimage = (unsigned char *)malloc(800*800*sizeof(unsigned char) ); 
	simper = (unsigned char *)malloc(800*800*sizeof(unsigned char) );
	memset(diffimage ,0 ,800*800*sizeof(unsigned char) ) ;
	return 0 ;
}

int  releasemotion()
{
	if ( motionprev != 0 )
	{
		free(motionprev);
		motionprev = 0;
	}
	if ( diffimage != 0 )
	{
		free(diffimage);
		diffimage = 0;
	}
	if ( simper != 0 )
	{
		free(simper);
		simper = 0;
	}

	return 0;
}

#if 1 


int  _subabs4(int  a,int  b)
{
	int32 x;
	union reg32 a32,b32,y32;

	a32.x1u = a;
	b32.x1u = b;

	x = (int32)a32.x4u.hi2-(int32)b32.x4u.hi2;
	if(x < (MIN_INT8 << 1))
		y32.x4u.hi2 = MAX_UINT8;
	else if(x < 0)
		y32.x4u.hi2 = (uint8)(-x);
	else
		y32.x4u.hi2 = (uint8)x;

	x = (int32)a32.x4u.hi1-(int32)b32.x4u.hi1;
	if(x < (MIN_INT8 << 1))
		y32.x4u.hi1 = MAX_UINT8;
	else if(x < 0)
		y32.x4u.hi1 = (uint8)(-x);
	else
		y32.x4u.hi1 = (uint8)x;

	x = (int32)a32.x4u.lo2-(int32)b32.x4u.lo2;
	if(x < (MIN_INT8 << 1))
		y32.x4u.lo2 = MAX_UINT8;
	else if(x < 0)
		y32.x4u.lo2 = (uint8)(-x);
	else
		y32.x4u.lo2 = (uint8)x;

	x = (int32)a32.x4u.lo1-(int32)b32.x4u.lo1;
	if(x < (MIN_INT8 << 1))
		y32.x4u.lo1 = MAX_UINT8;
	else if(x <  0)
		y32.x4u.lo1 = (uint8)(-x);
	else
		y32.x4u.lo1 = (uint8)x;

	return(y32.x1);
} /* end of _subabs4() function */

unsigned int _cmpgtu4(unsigned int src1, unsigned int src2)
{
	unsigned int result = 0;

	if ((src1 & 0xff) > (src2 & 0xff))
		result |= 8;
	if ((src1 & 0xff00) > (src2 & 0xff00))
		result |= 4;
	if ((src1 & 0xff0000) > (src2 & 0xff0000))
		result |= 2;
	if ((src1 & 0xff000000) > (src2 & 0xff000000))
		result |= 1;

	return (result);
}

int   _amem4(unsigned char  *a)
{


	int  a4 = (int )a[3]  <<24 & 0xFF000000;

	int  a5 = (int )a[2]  <<16 & 0xFF0000;
	int a6 = (int )a[1]  <<8 & 0xFF00;
	int a7 = (int )a[0]   & 0xFF;


	return    a4+a5+a6+a7;
}

#endif 

int    Diff_Threshold_INT     =   0x09090909 ;

#define  BINARY_WHITE_VALUE  1 


int   sub_binimage(unsigned char  *preGray,unsigned char  *thisGray, int nwidth ,int nheight ,unsigned char  *pDBit)
{
	int i,j,k ;
	int res;

	k =  0;
	//帧差 // 必须是4的倍数
	for ( j =  0 ; j<nheight ; j++)
	{        
		for ( i = 0; i<nwidth; i+= 4 )
		{         
			res= _cmpgtu4 (_subabs4(  *(int *)(preGray+j * nwidth+i),*(int *)(thisGray+j * nwidth+i)), Diff_Threshold_INT);
			*(int*)(pDBit+k)=((0x08&res)<<21) | ((0x04&res)<<14) | ((0x02&res)<<7) | ((0x01&res));
			k+=4;
		}
	}
	return 1;
}

#define M  8

#define N   7

#define  NUM_MAX_COUTOURS 64 


bool findcorners(unsigned char  * mid_p,int _w, int _h,PRECT  chain20,int *num_coutours)
{
	char chainway[M][N];
	//下一个指针偏移
	short ChainIndex[8];
	short ChainNext[M][2]={1,0,
		1,-1,
		0,-1,
		-1,-1,
		-1,0,
		-1,1,
		0,1,
		1,1};

	short i,j,k;// 迭代用
	//复制使用指针


	int point_ij=_w;
	int now_ij=0;
	int end_ij=_h*_w-2*_w; //引轮廓出洞
	short Now_way=4;
	int index_ij=0;
	short cnt=0;
	short nextway;

	short now_i;        //y
	short now_j;      //x
	short max_x;                   //最大x坐标
	short min_x;                    //最小x坐标
	short max_y;                   //最大y坐标
	short min_y;                    //最小y坐标
	short cnt_chain=0;               //链码条数计数


	ChainIndex[0]=  1;
	ChainIndex[1]=  1-_w;
	ChainIndex[2]=  -_w;
	ChainIndex[3]=  -1-_w;
	ChainIndex[4]=  -1;
	ChainIndex[5]=  -1+_w;
	ChainIndex[6]=  _w;
	ChainIndex[7]=  1+_w;

	for( i = 0;i<M;i++)
	{
		for( j = 0;j<N;j++)
		{
			chainway[i][j]=(i-2+j+8)%8;
			//chainway[i][j]=(i-3+j+8)%8;
		}
	}

	//四周置0 防止越界
	for( i = 0;i< _h;i++)
	{
		*(mid_p+i*_w)=0;
		*(mid_p+(i+1)*_w-1)=0;
	}
	//上下框
	memset(mid_p,0,_w);
	memset(mid_p+(_h-3)*_w,0,_w);
	memset(mid_p+(_h-2)*_w,0,_w);
	memset(mid_p+(_h-1)*_w,0,_w);
	//memset(mid_p,0,_h*_w);


	*(mid_p+(_h-2)*_w+4)=1;
	*(mid_p+(_h-2)*_w+5)=1;
	*(mid_p+(_h-2)*_w+6)=1;
	*(mid_p+(_h-2)*_w+7)=1;
	*(mid_p+(_h-2)*_w+8)=1;
	*(mid_p+(_h-2)*_w+9)=1;
	*(mid_p+(_h-2)*_w+10)=1;
	*(mid_p+(_h-2)*_w+11)=1;

	//初始化
	point_ij=_w;
	now_ij=0;
	now_j=0;
	Now_way=4;
	index_ij=0;
	cnt=0;
	nextway=0;  


	while(point_ij < end_ij)
	{
		//5月9日
		for(;0x01010101!=_amem4(mid_p+point_ij);point_ij+=4);

		k=0;
		now_ij=point_ij;
		cnt=0;

		now_i=point_ij/_w;        		//y
		now_j=point_ij%_w;      		//x

		//5月9日
		memset(mid_p+point_ij-now_j,0,now_j);

		max_x=now_j;                   //最大x坐标
		min_x=now_j;                    //最小x坐标
		max_y=now_i;                   //最大y坐标
		min_y=now_i;                    //最小y坐标

		do{	
			for(k=0;k<N;k++)
			{
				//遍历查找各个方向
				//char dir=chainway[]
				nextway=chainway[Now_way][k];
				index_ij=now_ij+ChainIndex[nextway];//8个方向，逆时针
				// 判断哪个方向为1
				if(BINARY_WHITE_VALUE==*(mid_p+index_ij))
				{			
					now_ij=index_ij;
					Now_way=nextway;
					cnt++;
					now_i+=ChainNext[Now_way][1];
					now_j+=ChainNext[Now_way][0];

					max_x = now_j>max_x?now_j:max_x;
					min_x = now_j<min_x?now_j:min_x;
					max_y=now_i>max_y?now_i:max_y;	
					min_y=now_i<min_y?now_i:min_y;
					//停止找轮廓
					k = N;
				} 
			}

		}while( point_ij!=now_ij &&  (k<N || k==N+1) &&cnt<10000);    //只要一动
		//找轮廓的矩形边界

		//将图填充0画矩形框
		for(i=min_y;i<=max_y;i++)
		{
			/*for(j=min_x;j<=max_x;j++)
			{
			*(mid_p+i*_w+j)=0;
			}*/
			memset(mid_p+i*_w+min_x,0,max_x-min_x+1);

		}


		chain20[cnt_chain].left =min_x;
		chain20[cnt_chain].right =max_x;
		chain20[cnt_chain].top=min_y;
		chain20[cnt_chain].bottom =max_y;
		//chain20[cnt_chain].length=cnt;
		//只有链码长度大于一定值才加
		cnt_chain = cnt> 16 ? cnt_chain+1:cnt_chain;
		//if(NUM_MAX_COUTOURS == cnt_chain)
		//	break;
		point_ij=( NUM_MAX_COUTOURS == cnt_chain)?(_h*_w):point_ij;
		point_ij+=4;
	} 

	*num_coutours=cnt_chain;
	return 1;
}

float getpsnr(SIZE sz, unsigned char* pGray, PRECT pRect, unsigned char* pResult )
{
	//	PBYTE pImg = pResult;

	int sw = (pRect->right - pRect->left) >> 1;
	int sh = (pRect->bottom - pRect->top) >> 1;

	int w = sz.cx;
	int dw = (w - sw) * 2;

	unsigned char* p = pGray + (pRect->top * w) + pRect->left;

	int mse = 0;
	int sum_gray = 0;
	short max_gray = 0;
	int ncount1 = 0;
	int ncount2  = 0;

	for (int y=0; y<sh; y++) 
	{
		for (int x=0; x<sw; x++) 
		{
			short v0 = *pResult;

			short v1 = (short)((p[0] + p[1] + p[w] + p[w+1]) >> 2);
			short delta = (short)(v0 - v1);

			sum_gray += v1;
			mse += (delta * delta);



#ifdef DSP_PLATFORM
			max_gray = _max2(max_gray, v0);
			max_gray = _max2(max_gray, v1);
#else
			if (max_gray < v0) max_gray = v0;
			if (max_gray < v1) max_gray = v1;
#endif
			*pResult++ = (unsigned char)v1;
			p += 2;
		}

		p += dw;
	}

	int size = sw * sh;
	float rms = (float)sqrt(mse / (float)size);
	if (rms < 1)
	{
		return 255;
	}

	float psnr = (float)((float)8.685890 * log(max_gray / rms));
	return (psnr);
}


int RectCross(RECT rect1,RECT rect2, int num)
{
	if(abs((rect1.right + rect1.left )-(rect2.right + rect2.left)) <= (rect1.right + rect2.right - rect1.left -rect2.left + 4*num)
		&&abs((rect1.bottom + rect1.top)-(rect2.bottom + rect2.top)) <= (rect1.bottom +rect2.bottom - rect1.top -rect2.top + 4*num)
		)
	{
		return 1;
	}
	else
	{
		return 0;
	}
}


int frameno = 0 ;

int  motiondetect(unsigned char *pImage ,int width ,int height , int left, int right, int top , int bottom ,int day ,PRECT moverect ,int *movenum  )
{
	int  i ,j ;
	SIZE sz ;
	sz.cx = width ;
	sz.cy = height ;
	RECT  detectrect ;
	detectrect.left = left;
	detectrect.right = right;
	detectrect.top =    top  ;
	detectrect.bottom = bottom ;
	int smperwidth = width >>1 ;
	int  smperheight = height;
	*movenum = 0;
	int  daynight = 0;
	float psn =  getpsnr( sz,  pImage, &detectrect, simper  );

	if(frameno == 0)
	{
		memcpy( motionprev ,simper ,smperwidth*smperheight   ) ; 
		frameno = 1;
	}

#if TAO_SHOW//
	/*cv::Mat show1( height, width, CV_8UC1, (void*)simper );
	cv::imshow("pre",show1);*/
#endif

#if 0
	if(psn  > 40) 
	{
		*movenum = 0;
	//	memcpy( motionprev ,pImage ,width *height  ) ; 
		memcpy( motionprev ,simper ,smperwidth*smperheight   ) ; 
		return 0 ;
	}
#endif 
	if (day == 0)
	{
		Diff_Threshold_INT     =   0x06060606 ;
	}
	else
	{
		Diff_Threshold_INT     =   0x03030303;
	}

#if TAO_SHOW//
	cv::Mat show1( smperheight, smperwidth, CV_8UC1, (void*)simper );
	printf("move nums: %d\n", *movenum );

	cv::Mat show2( smperheight, smperwidth, CV_8UC1, (void*)motionprev );
	printf("move nums: %d\n", *movenum );

	//for ( int jj = 0; jj < show.rows*show.cols; jj++ )
	//{
	//	show.data[ jj ] *= 255;
	//}
	//for ( int xk = 0; xk < *movenum; xk++ )
	//{
	//	//cv::rectangle(show, cv::Rect( moverect[xk].left,  moverect[xk].top,  moverect[xk].right-  moverect[xk].left, moverect[xk].bottom- moverect[xk].top ),cv::Scalar(255),2 );
	//}
	cv::imshow("c",show1);
	cv::imshow("d",show2);
	//cvWaitKey();
#endif

	// diff image 
	sub_binimage(motionprev ,simper ,smperwidth  ,smperheight  ,diffimage) ;
	memcpy( motionprev ,simper ,smperwidth*smperheight   ) ; 
	 
	int movecount = 0;
	findcorners(diffimage,smperwidth  ,smperheight   ,moverect, movenum) ;
	movecount = * movenum;
	for(i=0; i<movecount; i++)
	{
		for(j=0; j<movecount; j++)
		{
			if(i!=j)
			{
				if(RectCross(moverect[i],moverect[j], 7))
				{
					moverect[i].left   = min(moverect[i].left,moverect[j].left);
					moverect[i].top    = min(moverect[i].top,moverect[j].top);
					moverect[i].right  = max(moverect[i].right,moverect[j].right);
					moverect[i].bottom = max(moverect[i].bottom,moverect[j].bottom);
					movecount--;
					memcpy(&moverect[j], &moverect[movecount], sizeof(moverect[movecount]));
					j = 0;

				}
			}
		}
	}
	for(i=0; i<movecount; i++)
	{
		moverect[i].right = moverect[i].right << 1 ;
		moverect[i].left = moverect[i].left << 1 ;
		moverect[i].top = (moverect[i].top+ (top>>1)  ) << 1 ;
		moverect[i].bottom = (moverect[i].bottom +(top >>1))<< 1 ;
	}

	//extension rect
	for(i=0; i<movecount; i++)
	{
		moverect[i].right = min( moverect[i].right + 10, width);
		moverect[i].left = max(moverect[i].left - 10, 0);
		moverect[i].top = max( moverect[i].top - 10, 0) ;
		moverect[i].bottom = min(moverect[i].bottom + 10,height);
	}

#if   0
	for(i=0; i<movecount; i++)
	{
		if(moverect[i].right - moverect[i].left < 0.1  * smperwidth ||
			moverect[i].bottom - moverect[i].top < 0.1  * smperheight  )
		{
			movecount-- ;
			memcpy(&moverect[i], &moverect[movecount], sizeof(moverect[movecount]));
		}
	}
#endif 
		
	*movenum = movecount ;
	return  movecount ;
}
